#ifndef PINHOLE_CAMERA_STEREO_H_
#define PINHOLE_CAMERA_STEREO_H_

#include <Eigen/Dense>
#include "utils/math_utils.h"
#include <opencv2/opencv.hpp>
#include <ros/ros.h>

namespace sdd_vio
{
	class PinholeCameraStereo
	{
	public:
		cv::Mat rectify_map_[2][2];
		cv::Mat Q_;													  // 4x4矩阵，将视差图像转换成深度图像
		cv::Ptr<cv::StereoSGBM> matcher_sgbm_400_, matcher_sgbm_200_; // 获取视差图像：块匹配
		cv::Ptr<cv::StereoBM> matcher_bm_400_;
		cv::Ptr<cv::StereoBM> matcher_bm_200_;
		cv::Ptr<cv::StereoBM> matcher_bm_100_;
		cv::Ptr<cv::StereoBM> matcher_bm_50_;
		bool rectify_maps_initialized_;
		double cx_rec_, cy_rec_, f_rec_, base_;
		Eigen::Matrix3f K_rec_;
		std::vector<double> cx_rec_pyr_, cy_rec_pyr_, f_rec_pyr_;
		vector_aligned<Eigen::Matrix3f> K_rec_pyr_;
		int frame_width_, frame_height_;
		int nlays_, base_layer_;

	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

		PinholeCameraStereo(const ros::NodeHandle &nh);

		inline bool isRectified() { return rectify_maps_initialized_; }
		inline double getf() { return f_rec_; }
		inline int getWidth() { return frame_width_; }
		inline int getHeight() { return frame_height_; }

		void undistortImage(const cv::Mat &raw0, cv::Mat &rectified0, const cv::Mat &raw1, cv::Mat &rectified1);
		void get3DMap(const cv::Mat &img0, const cv::Mat &img1, cv::Mat &depth);
		void getDisparityMap(const cv::Mat &img0, const cv::Mat &img1, cv::Mat &disp);

		// 三角化得到3D点
		void get3DPoints(const vector_aligned<Eigen::Vector2i> &feat_pixels,
						 const std::vector<float> &disp_vec, vector_aligned<Eigen::Vector3f> &feat_3D, int npts, int ilays);

		// 投影得到2D点
		void get2DPixels(const vector_aligned<Eigen::Vector3f> &feat_3D,
						 vector_aligned<Eigen::Vector2f> &feat_pixels, int npts, const int ilays);

		void configPyramid();						 // 配置图像金字塔
		void initPyramid(int nlays, int base_layer); // 初始化图像金字塔

	};
} // namespace sdd_vio

#endif /* PINHOLE_CAMERA_STEREO_H_ */
